#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

// 全局变量用于存储处理后的点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud;
std::string pcd_path, pcd_topic;

// 回调函数：用于定时发布点云
void publishCloudCallback(const ros::TimerEvent &event, ros::Publisher &pcd_pub)
{
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*filtered_cloud, output);
    output.header.frame_id = "laser";
    output.header.stamp = ros::Time::now(); // 每次发布都更新时间戳
    pcd_pub.publish(output);
    ROS_INFO("Published point cloud on topic %s", pcd_topic.c_str());
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pcd_reader");
    ros::NodeHandle nh;

    // 获取参数
    double down_sample;
    nh.param<std::string>("pcd_path", pcd_path, "");
    nh.param<std::string>("pcd_topic", pcd_topic, "/pcd_output");
    nh.param<double>("down_sample_size", down_sample, 0.0);

    if (pcd_path.empty())
    {
        ROS_ERROR("PCD file path is empty!");
        return -1;
    }

    // 加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile(pcd_path, *cloud) == -1)
    {
        ROS_ERROR("Failed to load PCD file: %s", pcd_path.c_str());
        return -1;
    }
    ROS_INFO("Loaded %zu points from %s", cloud->size(), pcd_path.c_str());

    // 点云降采样
    filtered_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    if (down_sample > 0.0)
    {
        pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
        voxel_filter.setInputCloud(cloud);
        voxel_filter.setLeafSize(down_sample, down_sample, down_sample);
        voxel_filter.filter(*filtered_cloud);
        ROS_INFO("Downsampled to %zu points", filtered_cloud->size());
    }
    else
    {
        *filtered_cloud = *cloud;
    }

    // 创建发布者
    ros::Publisher pcd_pub = nh.advertise<sensor_msgs::PointCloud2>(pcd_topic, 1, true);

    // 创建定时器，每5秒触发一次回调函数
    ros::Timer timer = nh.createTimer(ros::Duration(5.0), boost::bind(publishCloudCallback, _1, boost::ref(pcd_pub)));

    // 启动ROS主循环
    ros::spin();

    return 0;
}